/*
 * toolbox.c
 *
 *  Created on: May 7, 2010
 *      Author: Armen Nazarian
*
 *
 * This cfile is for funtions that are NOT tasks and only purpose is to help
 * tasks calculate things. Helper-funtions
 *
 */


#include <includes.h>
#include "toolbox.h"
#include <../lego_interface/lego_interface.h>

/*
 * This functions converts the parameter num, (INT8U) to its binary value and it
 * is represented by the 8 diodes on the STK600 board.
 */


void convertToBinaryFunctionAndDisplay(INT8U num) {
	turn_all_diodes_off();
	int i;

	for (i = 1; i <= 8; i++) {

		if ((num & (1 << (i - 1))) == 0) {
			LED_Off(i);
		} else {
			LED_On(i);
		}
	}

}

/*
 * This function just turns all the diodes off
 */
void turn_all_diodes_off() {
	int i;
	for (i = 1; i <= 8; i++) {
		LED_Off(i);
	}
}
/*
 * This funtion stops the 1. belt immidiatly.
 * Unlike the method first_belt_run() task in legotasks.c, this is a hard stop
 * where the other is a soft stop, where a flag first have to be set.
 */
INT8U emergency_stop_first_belt() {
	motor_speed(1, 0);
	return 0;
}

/*
 * This funtion runs the 1. belt immidiatly.
 * Unlike the method first_belt_run() task in legotasks.c, this is a hard run
 * where the other is a soft run, where a flag first have to be set.
 */

INT8U emergency_over_first_belt() {
	motor_speed(1, 60);
	return 1; // returns a INT8U so the user of this function can set the On_flag_first_belt
	// so its doesn't run after this method is finished.
}

/*This function runs the sorter, with speed 8*/
void brick_sorter_run() {
	motor_speed(3, -16);

}
/*This function stops the sorter*/
/*the reason why it doesn't use the brake method for the motor is that the sorter
 * should have free run, so it can stop after the touch sensor and not on it.
 */

void brick_sorter_halt() {
	//OSTimeDly(OS_TICKS_PER_SEC / 5);
	motor_speed(3, 0);

}
